#include "stm32f10x.h"                  // Device header
#include "MyI2C.h"
#include "MPU6050_Reg.h"
#include "MPU6050.h"
#define MPU6050_ADDRESS		0xD0
#define scale_factor 2048.0f // 对应 ±16g 的加速度计范围
#define g_to_mps2 9.80665f // 重力加速度转换为 m/s²
#define scale_factor_gyro 16.4f // 对应 ±2000°/s 的陀螺仪范围
void MPU6050_WriteReg(uint8_t RegAddress, uint8_t Data)
{
	MyI2C_Start();
	MyI2C_SendByte(MPU6050_ADDRESS);
	MyI2C_ReceiveAck();
	MyI2C_SendByte(RegAddress);
	MyI2C_ReceiveAck();
	MyI2C_SendByte(Data);
	MyI2C_ReceiveAck();
	MyI2C_Stop();
}

uint8_t MPU6050_ReadReg(uint8_t RegAddress)
{
	uint8_t Data;
	
	MyI2C_Start();
	MyI2C_SendByte(MPU6050_ADDRESS);
	MyI2C_ReceiveAck();
	MyI2C_SendByte(RegAddress);
	MyI2C_ReceiveAck();
	
	MyI2C_Start();
	MyI2C_SendByte(MPU6050_ADDRESS | 0x01);
	MyI2C_ReceiveAck();
	Data = MyI2C_ReceiveByte();
	MyI2C_SendAck(1);
	MyI2C_Stop();
	
	return Data;
}

void MPU6050_Init(void)
{
	MyI2C_Init();
	uint8_t id = MPU6050_GetID();
	if (id != 0x68) {	
		printf("MPU6050 not detected! ID: 0x%02X\n", id);
		return;
	}
	MPU6050_WriteReg(MPU6050_PWR_MGMT_1, 0x01);
	MPU6050_WriteReg(MPU6050_PWR_MGMT_2, 0x00);
	MPU6050_WriteReg(MPU6050_SMPLRT_DIV, 0x09);
	MPU6050_WriteReg(MPU6050_CONFIG, 0x06);
	MPU6050_WriteReg(MPU6050_GYRO_CONFIG, 0x18);
	MPU6050_WriteReg(MPU6050_ACCEL_CONFIG, 0x18);
}

uint8_t MPU6050_GetID(void)
{
	return MPU6050_ReadReg(MPU6050_WHO_AM_I);
}

void mpu6050_get_data(MPU6050_Data* data)
{
	uint8_t DataH, DataL;
	int16_t ax, ay, az;
	int16_t gx, gy, gz;
	
	DataH = MPU6050_ReadReg(MPU6050_ACCEL_XOUT_H);
	DataL = MPU6050_ReadReg(MPU6050_ACCEL_XOUT_L);
	ax = (int16_t) ((DataH << 8) | DataL);
	
	DataH = MPU6050_ReadReg(MPU6050_ACCEL_YOUT_H);
	DataL = MPU6050_ReadReg(MPU6050_ACCEL_YOUT_L);
	ay = (int16_t) ((DataH << 8) | DataL);
	
	DataH = MPU6050_ReadReg(MPU6050_ACCEL_ZOUT_H);
	DataL = MPU6050_ReadReg(MPU6050_ACCEL_ZOUT_L);
	az = (int16_t) ((DataH << 8) | DataL);
	
	DataH = MPU6050_ReadReg(MPU6050_GYRO_XOUT_H);
	DataL = MPU6050_ReadReg(MPU6050_GYRO_XOUT_L);
	gx = (int16_t) ((DataH << 8) | DataL);
	
	DataH = MPU6050_ReadReg(MPU6050_GYRO_YOUT_H);
	DataL = MPU6050_ReadReg(MPU6050_GYRO_YOUT_L);
	gy = (int16_t) ((DataH << 8) | DataL);

	DataH = MPU6050_ReadReg(MPU6050_GYRO_ZOUT_H);
	DataL = MPU6050_ReadReg(MPU6050_GYRO_ZOUT_L);
	gz = (int16_t) ((DataH << 8) | DataL);
	// printf("Raw Accel: X=%d, Y=%d, Z=%d\n", ax, ay, az);
	// printf("Raw Gyro : X=%d, Y=%d, Z=%d\n", gx, gy, gz);


	// 将原始数据转换为实际值
	data->accel_x = (float)ax/scale_factor * g_to_mps2;
    data->accel_y = (float)ay/scale_factor * g_to_mps2;
    data->accel_z = (float)az/scale_factor * g_to_mps2;

    data->gyro_x = (float)gx/scale_factor_gyro;
    data->gyro_y = (float)gy/scale_factor_gyro;
    data->gyro_z = (float)gz/scale_factor_gyro;
	// printf("Accel: X=%.2f m/s², Y=%.2f m/s², Z=%.2f m/s²\n", data->accel_x, data->accel_y, data->accel_z);
	// printf("Gyro : X=%.2f °/s, Y=%.2f °/s, Z=%.2f °/s\n", data->gyro_x, data->gyro_y, data->gyro_z);
}
